/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "JPEGEncoder.hpp"
#include "TOccupancyGrid.h"
#include "glutImageGUI.h"
#include <unistd.h>
#include <string>

void printUsage(){
		fprintf(stderr,"####################################################################################\n");
		fprintf(stderr,"\toccu_tool. A tool for handling Occupancy Grid Maps.\n\n");
		fprintf(stderr,"\tusage: ./occu_tool -i [in] -o [out] -M [num] [opts]\n");
		fprintf(stderr,"\t\t-M [num]: Mode for the tool\n");
		fprintf(stderr,"\t\t\t-M 0 Change resolution (use -r to give output resolution)  \n");
		fprintf(stderr,"\t\t\t-M 1 Crop (around origin, use -s to give the size) \n");
		fprintf(stderr,"\t\t\t-M 2 Crop (around given point, use -s to give the size, -x for xcoordinate and -y for y) \n");
		fprintf(stderr,"\t\t\t \n");
		fprintf(stderr,"\t\t\t \n");
		
		
		fprintf(stderr,"\t\t-i [in]: Input map name (note: Occumap name) \n");
		fprintf(stderr,"\t\t-o [out]: Output file name\n");
		fprintf(stderr,"\t\t-r [reso] The resolution (output)\n");
		fprintf(stderr,"\t\t-j [name] Save output as JPEG image\n");
		fprintf(stderr,"\t\t-s [size] Size of the map in meters\n");
		fprintf(stderr,"\t\t-x [m] x-coordinate\n");
		fprintf(stderr,"\t\t-y [m] y-coordinate\n");
		fprintf(stderr,"\t\t\n");
		fprintf(stderr,"\n");
		fprintf(stderr,"\n");
		fprintf(stderr,"####################################################################################\n");
}

void saveGrid(TOccupancyGrid *O, const char *fname){
		CJPEGEncoder encoder;
		unsigned char *data = (unsigned char *) malloc(O->size());
		float *dptr = O->getDataPtr();
		int Nx,Ny;
		O->getDim(Nx,Ny);
						
		for(int i=0;i<O->size();i++) data[i] = (unsigned char) (dptr[i]*255);
		
		encoder.SetGrayscaleDataPtr((const char *) data,Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
					 encoder.GetJPEGDataSize(), 
					 1, f);
		fclose(f);
		
}

/**
* Change resolution
*/
bool changeResolution(std::string infile, 
											std::string outfile, 
											float outreso, 
											bool saveJpeg, 
											std::string jpegname)
{
		TOccupancyGrid O;
		O.load(infile.c_str());
		
		int Nx,Ny;
		float resolution, size_x,size_y;
		O.getDim(Nx,Ny);
		resolution = O.getResolution();
		
		size_x = ((float)Nx*resolution);
		size_y = ((float)Ny*resolution);
		
		Grid::TCoord2D orig =  O.getWorldPos();
		
		TOccupancyGrid Oout(size_x,size_y,outreso,orig);
		Oout.setGridToValue(0.5);
		
		Grid::TPixel pix;
		grid_t v;		
		for(int i=0;i<O.size();i++){
			if(O.getValue(Grid::TPixel(i%Nx,i/Nx),v)){
					Oout.updateProbability(O.toCoord(Grid::TPixel(i%Nx,i/Nx)), v);
			}
		}
		if(saveJpeg) saveGrid(&Oout,jpegname.c_str());
		
		Oout.save(outfile.c_str());
		return true;
}

/**
* crop the map around origin
*/
bool cropMap(std::string infile, 
															std::string outfile, 
															float outsize, 
															bool saveJpeg, 
															std::string jpegname)
{
		TOccupancyGrid O;
		O.load(infile.c_str());
		float resolution = O.getResolution();
		Grid::TCoord2D orig =  O.getWorldPos();
		fprintf(stderr,"reso = %.2f, size = %.2f\n",resolution,outsize);
		TOccupancyGrid Oout(outsize,outsize,resolution,orig);
		Oout.setGridToValue(0.5);
		float *dptr = Oout.getDataPtr();
		int Nx,Ny;
		Oout.getDim(Nx,Ny);
		O.getSubmap(dptr, O.toPixel(orig) , Nx, Ny);

		if(saveJpeg) saveGrid(&Oout,jpegname.c_str());
		
		Oout.save(outfile.c_str());
		return true;
}

/**
* crop the map around origin
*/
bool cropMapAroundCoord(std::string infile, 
															std::string outfile, 
															float outsize, 
															bool saveJpeg, 
															std::string jpegname, 
															float x, 
															float y)
{
		fprintf(stderr,"Crop Around the Block\n");
		TOccupancyGrid O;
		O.load(infile.c_str());
		float resolution = O.getResolution();
		Grid::TCoord2D orig(x,y);
		fprintf(stderr,"reso = %.2f, size = %.2f\n",resolution,outsize);
		TOccupancyGrid Oout(outsize,outsize,resolution,orig);
		Oout.setGridToValue(0.5);
		float *dptr = Oout.getDataPtr();
		int Nx,Ny;
		Oout.getDim(Nx,Ny);
		O.getSubmap(dptr, O.toPixel(orig) , Nx, Ny);

		if(saveJpeg) saveGrid(&Oout,jpegname.c_str());
		
		Oout.save(outfile.c_str());
		return true;
}



int main(int argc, char *argv[]){
	int mode = 0;
	bool saveJpeg=false;
	std:string  jpegname;
	std::string infilename="noname";
	std::string outfilename="noname";
	float out_resolution=0.1;
	float size=10.0;
	float x=0;
	float y=0;
	// Parse parameters
	bool parse = true;
	while(parse) {
			int c;
			c = getopt(argc, argv, "i:o:M:r:j:s:x:y:h");
			switch(c) {

					case 'i':{
							infilename = optarg;
							fprintf(stderr,"infile=%s\n",infilename.c_str());
							break;
					}
					case 'x':{
							x = atof(optarg);
							fprintf(stderr,"X=%f\n",x);
							break;
					}
					case 'y':{
							y = atof(optarg);
							fprintf(stderr,"Y=%f\n",y);
							
							break;
					}
					case 'o':{
							outfilename = optarg;
							break;
					}
					case 'M':{
								mode = atoi(optarg);
						}
						break;
					case 'r':{
							out_resolution = atof(optarg);
					}
					case 'j':{
							saveJpeg = true;
							jpegname = optarg;
					}
					break;
					case 's':{
							size=atof(optarg);
							fprintf(stderr,"Using %f as map size\n",size);
					}
					break;
					
					case 'h':
					case '?':
							printUsage();
							exit(1);
							break;
      
					case -1:
							parse = false;
							break;
			}
	}
	
	fprintf(stderr,"Select mode\n");
	
	//////////////////////////////////////////
	///MODE
	//////////////////////////////////////////
	switch(mode){
			case 0:{
					changeResolution(infilename, outfilename, out_resolution, saveJpeg, jpegname);
			}
			break;
			case 1: 
					cropMap(infilename, outfilename, size, saveJpeg, jpegname);
					break;
			case 2: 
					cropMapAroundCoord(infilename, outfilename, size, saveJpeg, jpegname,x,y);
					break;
			default: 
					fprintf(stderr,"ERROR!!!! Invalid Mode!!!\n");
					printUsage();
					exit(1);
					break;
					 
	}

	
return 0;
}
